//
//  Algorithm.cpp
//  newAR
//
//  Created by fg on 5/9/16.
//  Copyright © 2016 Shangjin Zhai. All rights reserved.
//

#include "Algorithm.hpp"
#include "LA.hpp"


using namespace cv;
using namespace std;

namespace misc {
    
    // Copyright 2000 softSurfer, 2012 Dan Sunday
    // This code may be freely used and modified for any purpose
    // providing that this copyright notice is included with it.
    // SoftSurfer makes no warranty for this code, and cannot be held
    // liable for any real or imagined damage resulting from its use.
    // Users of this code must verify correctness for their application.
    
    
    // a Point is defined by its coordinates {int x, y;}
    //===================================================================
    
    
    // isLeft(): tests if a point is Left|On|Right of an infinite line.
    //    Input:  three points P0, P1, and P2
    //    Return: >0 for P2 left of the line through P0 and P1
    //            =0 for P2  on the line
    //            <0 for P2  right of the line
    //    See: Algorithm 1 "Area of Triangles and Polygons"
    inline int
    isLeft( Vector2d P0, Vector2d P1, Vector2d P2 )
    {
        return ( (P1.x() - P0.x()) * (P2.y() - P0.y())
                - (P2.x() -  P0.x()) * (P1.y() - P0.y()) );
    }
    //===================================================================
    
    
    // cn_PnPoly(): crossing number test for a point in a polygon
    //      Input:   P = a point,
    //               V[] = vertex points of a polygon V[n+1] with V[n]=V[0]
    //      Return:  0 = outside, 1 = inside
    // This code is patterned after [Franklin, 2000]
    int
    cn_PnPoly( Vector2d P, Vector2d* V, int n )
    {
        int    cn = 0;    // the  crossing number counter
        
        // loop through all edges of the polygon
        for (int i=0; i<n; i++) {    // edge from V[i]  to V[i+1]
            if (((V[i].y() <= P.y()) && (V[i+1].y() > P.y()))     // an upward crossing
                || ((V[i].y() > P.y()) && (V[i+1].y() <=  P.y()))) { // a downward crossing
                // compute  the actual edge-ray intersect x-coordinate
                double vt = (P.y()  - V[i].y()) / (V[i+1].y() - V[i].y());
                if (P.x() <  V[i].x() + vt * (V[i+1].x() - V[i].x())) // P.x() < intersect
                    ++cn;   // a valid crossing of y=P.y() right of P.x()
            }
        }
        return (cn&1);    // 0 if even (out), and 1 if  odd (in)
        
    }
    //===================================================================
    
    
    // wn_PnPoly(): winding number test for a point in a polygon
    //      Input:   P = a point,
    //               V[] = vertex points of a polygon V[n+1] with V[n]=V[0]
    //      Return:  wn = the winding number (=0 only when P is outside)
    int
    wn_PnPoly( Vector2d P, Vector2d* V, int n )
    {
        int    wn = 0;    // the  winding number counter
        
        // loop through all edges of the polygon
        for (int i=0; i<n; i++) {   // edge from V[i] to  V[i+1]
            if (V[i].y() <= P.y()) {          // start y <= P.y()
                if (V[i+1].y()  > P.y())      // an upward crossing
                    if (isLeft( V[i], V[i+1], P) > 0)  // P left of  edge
                        ++wn;            // have  a valid up intersect
            }
            else {                        // start y > P.y() (no test needed)
                if (V[i+1].y()  <= P.y())     // a downward crossing
                    if (isLeft( V[i], V[i+1], P) < 0)  // P right of  edge
                        --wn;            // have  a valid down intersect
            }
        }
        return wn;
    }
    
    int distance(const cv::Mat &a,const cv::Mat &b)
    {
        // Bit set count operation from
        // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
        
        const int *pa = a.ptr<int32_t>();
        const int *pb = b.ptr<int32_t>();
        
        int dist=0;
        
        for(int i=0; i<8; i++, pa++, pb++)
        {
            unsigned  int v = *pa ^ *pb;
            v = v - ((v >> 1) & 0x55555555);
            v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
            dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
        }
        
        return dist;
    }
    
    class parallelMatch : public cv::ParallelLoopBody
    {
    public:
        parallelMatch(const std::vector<Feature*>&query,const std::vector<Feature*>&train,std::vector<DMatch>&dmatches,const std::vector<std::vector<int>> *bucketFeature,int width)
        {
            this->query = &query[0];
            this->train = &train[0];
            trainSize = train.size();
            dmatch = &dmatches[0];
            this->p = bucketFeature;
            this->width = width;
        };
        void operator() (const Range& range) const
        {
            
            for (int i=range.start; i<range.end; ++i) {
                int minDist = 256;
                int minj=-1;
                int x = query[i]->_pos.x()/10;
                int y = query[i]->_pos.y()/10;
                int idx = y*width+x;
                for (auto&j:(*p)[idx]) {
                    int dist = distance(query[i]->desp, train[j]->desp);
                    if (dist<minDist) {
                        minDist = dist;
                        minj = j;
                    }
                }
                dmatch[i].distance = minDist;
                dmatch[i].queryIdx = i;
                dmatch[i].trainIdx = minj;
            }
        }
    private:
        Feature* const* query;
        Feature* const* train;
        const std::vector<std::vector<int>> * p;
        int trainSize;
        DMatch* dmatch;
        int width;
    };
    
    double computeMeanReprojectEror(const IntrinsicMatrix &K,const Pose&p,const std::vector<Feature*>&fts)
    {
        double err = 0;
        for(int i=0;i<fts.size();++i)
        {
            Eigen::Matrix2d a;
            err +=(K.project(p.map(fts[i]->pointXYZ->pointXYZ)) - fts[i]->_pos).norm();
        }
        return err/fts.size();
    }
    
    
    void matchRadius(const std::vector<Feature*>&query,const std::vector<Feature*>&train,std::vector<DMatch>&dmatches,cv::Size fullImgSize)
    {
        static vector<vector<int>> bucketIdxBucket;//store which bucket should be searched by current bucket
        static vector<vector<int>> bucketIdxFeature;//store feature idx for this bucket to match with
        static int WIDTH = 0;
        int width = fullImgSize.width/10;
        int height = fullImgSize.height/10;
        if (WIDTH == 0) {
            WIDTH = width;
        }
        assert(width == WIDTH);
        if(bucketIdxBucket.empty())
        {
            bucketIdxBucket.resize(width*height);
            bucketIdxFeature.resize(width*height);
            for (int i=1; i<height-1; ++i) {
                for (int j=1; j<width-1; ++j) {
                    int idx = i*width+j;
                    bucketIdxBucket[idx].reserve(9);
                    bucketIdxBucket[idx].push_back(idx);
                    bucketIdxBucket[idx].push_back(idx-1);
                    bucketIdxBucket[idx].push_back(idx+1);
                    bucketIdxBucket[idx].push_back(idx-width);
                    bucketIdxBucket[idx].push_back(idx-width-1);
                    bucketIdxBucket[idx].push_back(idx-width+1);
                    bucketIdxBucket[idx].push_back(idx+width);
                    bucketIdxBucket[idx].push_back(idx+width-1);
                    bucketIdxBucket[idx].push_back(idx+width+1);
                    bucketIdxFeature[idx].reserve(20);
                }
            }
            int idxs[4] = {0,width-2,width*(height-2),width*(height-1)-2};
            for (int i=0; i<4; ++i) {
                int idx = idxs[i];
                bucketIdxBucket[idx].reserve(4);
                bucketIdxBucket[idx].push_back(idx);
                bucketIdxBucket[idx].push_back(idx+1);
                bucketIdxBucket[idx].push_back(idx+width);
                bucketIdxBucket[idx].push_back(idx+width+1);
                bucketIdxFeature[idx].reserve(20);
            }
            int start[4]={1,1+width*(height-2),width,width*2-2};
            int end[4] = {width-1,width*(height-1)-2,width*(height-1),width*(height-1)-2};
            int step[4] = {1,1,width,width};
            int offset[4]={0,width,0,1};
            for (int i=0; i<2; ++i) {
                for (int idx = start[i]; idx<end[i]; idx+=step[i]) {
                    bucketIdxBucket[idx+offset[i]].reserve(6);
                    bucketIdxBucket[idx+offset[i]].push_back(idx-1);
                    bucketIdxBucket[idx+offset[i]].push_back(idx-1+width);
                    bucketIdxBucket[idx+offset[i]].push_back(idx);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+1);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+width);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+width+1);
                    bucketIdxFeature[idx+offset[i]].reserve(20);
                }
            }
            for (int i=2; i<4; ++i) {
                for (int idx = start[i]; idx<end[i]; idx+=step[i]) {
                    bucketIdxBucket[idx+offset[i]].reserve(6);
                    bucketIdxBucket[idx+offset[i]].push_back(idx);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+1);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+width);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+1+width);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+width*2);
                    bucketIdxBucket[idx+offset[i]].push_back(idx+width*2+1);
                    bucketIdxFeature[idx+offset[i]].reserve(20);
                }
            }
        }
        
        for (int i=0; i<bucketIdxFeature.size(); ++i) {
            bucketIdxFeature[i].clear();
        }
        
        for (int i=0; i<train.size(); ++i) {
            if (train[i]->isBad()) {
                continue;
            }
            int y = train[i]->_pos.y()/10;
            int x = train[i]->_pos.x()/10;
            int idx = y*width+x;
            for(auto &b :bucketIdxBucket[idx])
            {
                bucketIdxFeature[b].push_back(i);
            }
        }
        
        dmatches.resize(query.size());

        parallel_for_(cv::Range(0,query.size()), parallelMatch(query, train, dmatches,&bucketIdxFeature,width));
    }
    
    void warpMask(Mat &mask,cv::Size imgSize,const Eigen::Matrix3d &Hnm)
    {
        mask = Mat(imgSize,CV_8U);
        Eigen::Vector2d p[4],w[4];
        p[0][0] = 0;p[0][1] = 0;
        p[1][0] = imgSize.width;p[0][1] = 0;
        p[2][0] = imgSize.width;p[2][1] = imgSize.height;
        p[3][0] = 0;p[3][1] = imgSize.height;
        double maxx=-1,maxy=-1,minx=imgSize.width,miny=imgSize.height;
        
        for (int i=0; i<4; ++i) {
            applyHomography(Hnm, p[i][0], p[i][1], w[i][0], w[i][1]);
            minx = std::min(minx,w[i][0]);
            miny = std::min(miny,w[i][1]);
            maxx = std::max(maxx, w[i][0]);
            maxy = std::max(maxy, w[i][1]);
        }
        for (int i=0; i<imgSize.height; ++i) {
            auto p = mask.ptr<uchar>(i);
            for (int j=0; j<imgSize.width; ++j) {
                if (i<miny){
                    p[j] = 0;
                    continue;
                }
                if (i>maxy)
                {
                 p[j] = 0;
                    continue;
                }
                if (j<minx){
                    p[j] = 0;
                    continue;
                }
                if (j>maxx) {
                    p[j] = 0;
                    continue;
                }
                p[j] = 1;
            }
        }
    }
}